Visualization of Walking BSLIP in 3D simulation results

Last edits:

Sep. 30th, 2013 Moritz Maus
Oct. 1st, 2013 Moritz Maus: created alpha-L0-Plots
Oct. 28th, 2013 Moritz Maus: created stride visualization
Oct. 31th, 2013 Moritz Maus: improved visualization of selected solutions

Content:

Step 1: select and load data
Step 2: plot "type-1" graphs
Step 3: plot "type-2" graphs
Step 4: visualize selected solution

Step 1: configure notebook and load data

content



In [6]:

    
import sys
import numpy as np
import matplotlib

import mutils.io as mio
import mutils.misc as mi

c = mio.saveable() # config workspace
c.beta  = 5
c.beta_doc = 'beta angle of legs (in deg., 5 and 10 are available)'

c.point = 'B'
c.point_doc = 'which point of Geyers solutions (A,B,C)'

c.background_only = False
c.background_only_doc = 'display only background, no additional information'

c.display()

#dat1 = mio.mload('bslip_results/' + c.point + '_sols5_new.list')
#dat2 = mio.mload('bslip_results/' + c.point + '_sols10_new.list')
# dat is a list. each element is a list of the following 4 items:
#   + initial conditions (array)
#   + parameters: k1, k2, alpha1, alpha2, beta1, beta2, l1, l2 
#   + walking radius 
#   + eigenvalues of periodic solution (note: 2-step periodic!)

Step 2: plot "type-1" solutions: delta-k, delta-alpha plane

These solutions have k1 = k + $\Delta$k, k2 = k - $\Delta$k, and $\alpha_1 = \alpha + \Delta \alpha$, $\alpha_2 = \alpha - \Delta \alpha$

content
That is, we should visualize them as $\Delta k - \Delta \alpha$ - plots.

first step: load and order data



In [2]:

    
dat1 = mio.mload('bslip_results/' + c.point + '_sols5_new.list')
dat2 = mio.mload('bslip_results/' + c.point + '_sols10_new.list')

# compute range of axes
dk = array([x[1][0] - x[1][1] for x in dat1])
da = array([x[1][2] - x[1][3] for x in dat1])

nk1 = len(set(dk)) # number of different dk's
na1 = len(set(da)) # number of different da's

ax_a1 = linspace(min(da), max(da), na1)
ax_k1 = linspace(min(dk), max(dk), nk1)

# delta_k
dk = array([x[1][0] - x[1][1] for x in dat2])
da = array([x[1][2] - x[1][3] for x in dat2])

nk2 = len(set(dk)) # number of different dk's
na2 = len(set(da)) # number of different da's

ax_a2= linspace(min(da), max(da), na2)
ax_k2 = linspace(min(dk), max(dk), nk2)

print "re-ordering data into grid:"
n = 0
data1 = nan*zeros((nk1, na1))
print "ws1:"
for elem in dat1:
    # compute index
    dk = elem[1][0] - elem[1][1]
    da = elem[1][2] - elem[1][3]
    idx_a = argmin(abs(ax_a1 - da))
    idx_k = argmin(abs(ax_k1 - dk))
    data1[idx_k, idx_a] = elem[2]
    n+=1
    if n >= 500:       
        sys.stdout.write('.')        
        n=0
    
print "done"

n = 0
data2 = nan*zeros((nk2, na2))
print "ws2:"
for elem in dat2:
    # compute index
    dk = elem[1][0] - elem[1][1]
    da = elem[1][2] - elem[1][3]
    idx_a = argmin(abs(ax_a2 - da))
    idx_k = argmin(abs(ax_k2 - dk))
    data2[idx_k, idx_a] = elem[2]
    n+=1
    if n >= 500:       
        sys.stdout.write('.')        
        n=0
    
print "done"









    



re-ordering data into grid:
ws1:
.......done
ws2:
......done

second: create graphs



In [7]:

    
colorlim = [-200,200]
if c.point in 'AB':
    myxlim=[-5,0]
    myylim=[-20,20]
else:
    myxlim=[-12,0]
    myylim=[-2,2]
    
cmap = matplotlib.cm.Spectral
cmap.set_bad('w',1.)

if c.point=='A':
    levels = [-200,-100,-50,-20,-10,10,20,50,100,200]
    fig = figure(figsize=(10,6))
elif c.point=='B':
    levels = [-100,-20,-10,10,20,100]
    fig = figure(figsize=(5.5,4))    
else:
    levels = [-150,-50,-20,-10,10,20,50,150]
    fig = figure(figsize=(5.5,4))


#--------------------- general frame plot for common title etc.
#
ax0 = fig.add_subplot(1,1,1,frameon=False)
ax0.set_xticks([])
ax0.set_yticks([])


#--------------------- first plot
ax1 = fig.add_subplot(1,2,1)
masked_array1 = ma.array (data1.T, mask=np.isnan(data1.T))
pcl1 = ax1.pcolor(ax_k1 / 1000., ax_a1 * 180 / pi, masked_array1, cmap=cmap)
pcl1.set_clim(colorlim)
if not c.background_only:
    masked_array1ct = ma.array (data1.T, mask=abs(data1.T)>max(abs(array(colorlim)))*1.2)
    ct1 = ax1.contour(ax_k1 / 1000., ax_a1 * 180/pi, masked_array1ct, levels, linestyles='-',
                      colors='k', )
    clabel(ct1, ct1.levels, inline=True, fmt='%.0f', fontsize=10)


#--------------------- second plot
ax2 = fig.add_subplot(1,2,2)
masked_array2 = ma.array (data2.T, mask=np.isnan(data2.T))
pcl2 = ax2.pcolor(ax_k2 / 1000., ax_a2 * 180/pi, masked_array2, cmap=cmap)
pcl2.set_clim(colorlim)
if not c.background_only:
    masked_array2ct = ma.array (data2.T, mask=abs(data2.T)>max(abs(array(colorlim)))*1.2)
    ct2 = ax2.contour(ax_k2 / 1000., ax_a2 * 180/pi, masked_array2ct, levels, linestyles='-',
                      colors='k', )
    clabel(ct2, ct2.levels, rightside_up=False, fmt='%.0f', fontsize=10)

#-------------------- colorbar frame
if not c.background_only:
    if c.point == 'A':
        ax3 = fig.add_axes([.8, .55, .08, .4])
        cb = colorbar(pcl2, cax=ax3)
        cb.set_ticks(linspace(colorlim[0], colorlim[1], 5))
        cb.set_label('walking radius [m]', fontsize=14)


#-------------------- labeling
ax0.set_xlabel('\n'*2 + r'$\Delta k$ [kN / m]', fontsize=14)
ax1.set_ylabel(r'$\Delta \alpha$[deg]', fontsize=14)
ax1.set_title(r'$\beta = 5^\circ $', fontsize=16)
ax2.set_title(r'$\beta = 10^\circ $', fontsize=16)


#-------------------- formatting
ax2.set_yticklabels([]) 
ax1.set_xlim(myxlim)
ax2.set_xlim(myxlim[::-1])
ax1.set_ylim(myylim)
ax2.set_ylim(myylim)
if c.point=='A':
    fig.subplots_adjust(hspace=0, wspace=0.025, bottom=.1, right=.875, left=.10)
else:
    fig.subplots_adjust(hspace=0, wspace=0.025, bottom=.2, right=.875, left=.15)
if not c.background_only:
    ax1.grid('on')
    ax2.grid('on')


#-------------------- save figures

#savefig('tmp/walking_radius_ak_' + c.point + '.png', dpi=100)
#savefig('tmp/walking_radius_ak_' + c.point + '.tiff', dpi=600)
#savefig('tmp/walking_radius_ak_' + c.point + '.svg', dpi=50)
if c.background_only:
    savefig('tmp/walking_radius_ak_' + c.point + '_bg.png')
else:
    savefig('tmp/walking_radius_ak_' + c.point + '.pdf')

pass

Step 3: plot type-2 solutions: alpha-l0

alpha-l0 changes content

First: load and re-order data



In [22]:

    
dat1 = mio.mload('bslip_results/' + c.point + '_sols5_new_al0.list')
dat2 = mio.mload('bslip_results/' + c.point + '_sols10_new_al0.list')
print "done!"



In [23]:

    
# compute range of axes
dl = array([x[1][6] - x[1][7] for x in dat1])
da = array([x[1][2] - x[1][3] for x in dat1])

c.select_ev = False

nl1 = len(set(dl)) # number of different dk's
na1 = len(set(da)) # number of different da's

ax_a1 = linspace(min(da), max(da), na1)
ax_l1 = linspace(min(dl), max(dl), nl1)

# delta_k
dl = array([x[1][6] - x[1][7] for x in dat2])
da = array([x[1][2] - x[1][3] for x in dat2])

nl2 = len(set(dl)) # number of different dk's
na2 = len(set(da)) # number of different da's

ax_a2= linspace(min(da), max(da), na2)
ax_l2 = linspace(min(dl), max(dl), nl2)

print "re-ordering data into grid:"
n = 0
data1 = nan*zeros((nl1, na1))
print "ws1:"
for elem in dat1:
    # compute index
    dl = elem[1][6] - elem[1][7]
    da = elem[1][2] - elem[1][3]
    idx_a = argmin(abs(ax_a1 - da))
    idx_l = argmin(abs(ax_l1 - dl))
    if not c.select_ev:
        data1[idx_l, idx_a] = elem[2]
    else:
        data1[idx_l, idx_a] = max(abs(elem[3]))
    n+=1
    if n >= 500:       
        sys.stdout.write('.')
        n=0

if c.point == "C": #circumvent a sign flip in the original data: flip alpha, l0, r
    ax_a1 *= -1
    ax_l1 *= -1
    data1 *= -1
        
print "done"

n = 0
data2 = nan*zeros((nl2, na2))
print "ws2:"
for elem in dat2:
    # compute index
    dl = elem[1][6] - elem[1][7]
    da = elem[1][2] - elem[1][3]
    idx_a = argmin(abs(ax_a2 - da))
    idx_l = argmin(abs(ax_l2 - dl))
    if not c.select_ev:
        data2[idx_l, idx_a] = elem[2]
    else:
        data2[idx_l, idx_a] = max(abs(elem[3]))
    n+=1
    if n >= 500:       
        sys.stdout.write('.')
        n=0
    
print "done"









    



re-ordering data into grid:
ws1:
.....................................................................................................................................done
ws2:
............................................................................................................................................done

second: create graphs



In [24]:

    
colorlim = [-200,200] if not c.select_ev else [0, 1]
#colorlim = [0,1]
if c.point == 'A':
    myxlim=[.1,0]
    myylim=[-20,20]
elif c.point == 'B':
    myxlim=[.1,0]
    myylim=[-20,20]
    #myylim=[5,10]
elif c.point == 'C':
    myylim=[-2,2]
    myxlim=[.014,0]
else:
    raise NameError("Unknown operation point c.point: select A, B or C")
    
cmap = matplotlib.cm.Spectral
cmap.set_bad('w',1.)
levels = [-100,-50,-20,-10,10,20,50,100]


#--------------------- general frame plot for common title etc.
fig = figure(figsize=(9,6))
ax0 = fig.add_subplot(1,1,1,frameon=False)
ax0.set_xticks([])
ax0.set_yticks([])


#--------------------- first plot
ax1 = fig.add_subplot(1,2,1)
masked_array1 = ma.array (data1.T, mask=np.isnan(data1.T))
pcl1 = ax1.pcolor(ax_l1, ax_a1 * 180 / pi, masked_array1, cmap=cmap)
pcl1.set_clim(colorlim)
masked_array1ct = ma.array (data1.T, mask=abs(data1.T)>max(abs(array(colorlim)))*1.2)
ct1 = ax1.contour(ax_l1, ax_a1 * 180/pi, masked_array1ct, levels, linestyles='-',
                  colors='k', )
clabel(ct1, ct1.levels, fmt='%.0f', fontsize=10)


#--------------------- second plot
ax2 = fig.add_subplot(1,2,2)
masked_array2 = ma.array (data2.T, mask=np.isnan(data2.T))
pcl2 = ax2.pcolor(ax_l2, ax_a2 * 180/pi, masked_array2, cmap=cmap)
pcl2.set_clim(colorlim)
masked_array2ct = ma.array (data2.T, mask=abs(data2.T)>max(abs(array(colorlim)))*1.2)
ct2 = ax2.contour(ax_l2, ax_a2 * 180/pi, masked_array2ct, levels, linestyles='-',
                  colors='k', )
clabel(ct2, ct2.levels, fmt='%.0f', fontsize=10)

#-------------------- colorbar frame
ax3 = fig.add_axes([.8, .55, .08, .4])
cb = colorbar(pcl2, cax=ax3)
cb.set_ticks(linspace(colorlim[0], colorlim[1], 5))
cb.set_label('walking radius [m]')


#-------------------- labeling
ax0.set_xlabel('\n'*2 + r'$\Delta l$ [m]')
ax1.set_ylabel(r'$\Delta \alpha$[deg]')
ax1.set_title(r'$\beta = 5^\circ $')
ax2.set_title(r'$\beta = 10^\circ $')


#-------------------- formatting
ax2.set_yticklabels([]) 
ax1.set_xlim(myxlim)
ax2.set_xlim(myxlim[::-1])
ax1.set_ylim(myylim)
ax2.set_ylim(myylim)
fig.subplots_adjust(hspace=0, wspace=0.05, bottom=.15, right=.85)
ax1.grid('on')
ax2.grid('on')


#-------------------- save figures
pass
#savefig('tmp/walking_radius_al_' + c.point + '.png')
#savefig('tmp/walking_radius_al_' + c.point + '.svg')
#savefig('tmp/walking_radius_al_' + c.point + '.pdf')

Step 4: visualize selected solution

requires:

Step 1: select data
Step 2: load and preprocess data (first cell only)

to content



In [212]:

    
# --- select solution properties
c.vis_beta = .10 # one out of .05, .10
c.vis_da = 1.3 * pi / 180
c.vis_dk = 8000 # sign flip in here!


# --- prepare block
# --- --- import functions
import models.bslip as bslip

# --- --- define useful helper function
def find_idx(dk, da, sols):
    """
    returns the solution index which most closely matches dk, da
    
    """
    #dk = array([x[1][0] - x[1][1] for x in dat1])
    #da = array([x[1][2] - x[1][3] for x in dat1])
    dk_ref = 1e9
    da_ref = 1e9
    idx_ref = 0
    for idx, sol in enumerate(sols):
        dk_f = sol[1][0] - sol[1][1]
        da_f = sol[1][2] - sol[1][3]
        if abs(da_f - da) <= da_ref:
            if abs(abs(dk_f) - dk) <= dk_ref:
                da_ref = abs(da_f - da)
                dk_ref = abs(abs(dk_f) - dk)
                idx_ref = idx
                                
    return idx_ref



In [213]:

    
# --- identify closest solution in dataset
if c.point == "A":
    pr0 = array([14000, 14000, 69.*pi / 180, 69 * pi / 180, .05, -.05, 1., 1.])
elif c.point == "B":
    pr0 = array([14000, 14000, 72.5 *pi / 180, 72.5 * pi / 180, .05, -.05, 1., 1.])
elif c.point == "C":
    pr0 = array([20000, 20000, 76.5 *pi / 180, 76.5 * pi / 180, .05, -.05, 1., 1.])
else:
    raise ValueError("Wrong value for c.point (A, , or C)")
        
pr0[4] = c.vis_beta
pr0[5] = -c.vis_beta
if c.vis_beta == .05:
    dat = dat1
elif c.vis_beta == .1:
    dat = dat2
else:
    raise ValueError("Wrong value for c.vis_beta (.05 or .1)")

# --- --- find best solution, get corresponding parameters and IC
idx = find_idx(c.vis_dk, c.vis_da, dat)
dk, da = -diff(dat[idx][1])[[0,2]]
ICc = dat[idx][0]

# --- --- update model parameters
pr0a = pr0.copy()
pr0a[2] += da / 2.
pr0a[3] -= da / 2.
pr0a[0] += dk / 2.
pr0a[1] -= dk / 2.

pbase = bslip.demo_p
pbase['delta_beta'] = .0
P = bslip.pred_to_p(pbase, pr0a)


# --- --- some debug output
print "idx = ", idx 
print "dk:", dk, "(", c.vis_dk, ")"
print "da:", da * 180 / pi, "(", da, ") (", c.vis_da, ")"
print "r:", dat[idx][2]









    



idx =  2191
dk: -8000.0 ( 8000 )
da: 1.3 ( 0.0226892802759 ) ( 0.0226892802759 )
r: -35.3479353354



In [214]:

    
# --- make several steps

# --- --- create the model with parameters and IC
ICp = bslip.ICcircle_to_ICeuklid(ICc)
mdl = bslip.BSLIP_newTD(P, ICp)

# --- --- run the model
for rep in range(10): #26):
    _ = mdl.do_step()

    
# --- --- basic visualization
if False: # don't do that right now
    figure()
    f = bslip.vis_sim(mdl)
    f.axes[0].set_ylim(.97,.99)
    
    figure(figsize=(14,6))
    for ts,td, fs, fd in zip(mdl.t_ss_seq, mdl.t_ds_seq, mdl.forces_ss_seq, mdl.forces_ds_seq):
        plot(ts, fs[:,1],'r')
        plot(ts, fs[:,4],'r--')
        plot(td, fd[:,1],'r')
        plot(td, fd[:,4],'r--')
    
else:
    print "done!" # give at least some output
    
pass









    



done!

visualize a single stride



In [218]:

    
# --- visualize
# --- --- switch figure format
%config InlineBackend.figure_format = 'png' # svg or png

fig = figure(figsize=(6.5,5.5))
nsteps = 4


titlefont={'family' : 'Sans',
           'size' : 10,
           'weight' : 'normal',
            }


# single stance line format
ss_fmt = { 'color' : '#555555',
          'linewidth' : 1.,
          'linestyle' : '-',
          }

# double stance line format
ds_fmt = { 'color' : '#000000',
          'linewidth' : 1.5,
          'linestyle' : '-',
          }


# --- --- gather and rotate position data to walk "mostly right"
dx, dz = mdl.y_ss_seq[nsteps][0,[0,2]] - mdl.y_ss_seq[0][0,[0,2]]
dphi = arctan2(dz, dx)

rotmat = array([[cos(dphi), -sin(dphi)],[sin(dphi), cos(dphi)]]).T
#rotmat = eye(2)

dat_ss = [dot(rotmat, mdl.y_ss_seq[rep][:, [0,2]].T).T for rep in range(nsteps)]
dat_ds = [dot(rotmat, mdl.y_ds_seq[rep][:, [0,2]].T).T for rep in range(nsteps)]

# --- --- top subplot: top view
ax0 = fig.add_subplot(2,1,1)
for rep in range(nsteps):
    ax0.plot(dat_ss[rep][:,0], dat_ss[rep][:,1], **ss_fmt)
    ax0.plot(dat_ds[rep][:,0], dat_ds[rep][:,1], **ds_fmt)
        
    # un-rotated data
    #ax0.plot(mdl.y_ss_seq[rep][:,0], mdl.y_ss_seq[rep][:,2], 'r+')
    #ax0.plot(mdl.y_ds_seq[rep][:,0], mdl.y_ds_seq[rep][:,2], 'g+')
    

ax0.plot([dat_ss[0][0,0], dat_ds[nsteps-1][-1,0]],
         [dat_ss[0][0,1], dat_ds[nsteps-1][-1,1]], 'k--')
          

# feet
min_foot = 0
max_foot = 0
nfeet = nsteps//2 + 1
for rep in range(nfeet):
    foot1 = dot(rotmat, array(mdl.feet1_seq[::2][rep])[[0,2]])
    if foot1[1] > max_foot:
        max_foot = foot1[1]
    elif foot1[1] < min_foot:
        min_foot = foot1[1]
    plot(foot1[0], foot1[1], color='#000000', marker='d')
    if rep < nfeet - 1: # skip last 2nd foot
        foot2 = dot(rotmat, array(mdl.feet2_seq[1::2][rep])[[0,2]])
        plot(foot2[0], foot2[1], color='#aaaaaa', marker='d')
        if foot2[1] > max_foot:
            max_foot = foot2[1]
        elif foot2[1] < min_foot:
            min_foot = foot2[1]
                



ax0.set_title('top view', fontdict=titlefont)
ax0.set_xlabel('horizontal CoM position [m]', fontdict=titlefont)
ax0.set_ylabel('lateral CoM position[m]', fontdict=titlefont)


# --- --- bottom subplot: vertical motion

ax1 = fig.add_subplot(2,1,2)
ax1b = ax1.twinx() # for the vertical forces
mg = 80*9.81 # body weight
for rep in range(nsteps):
    ax1.plot(mdl.t_ss_seq[rep], mdl.y_ss_seq[rep][:,1], '-', **ss_fmt)
    ax1.plot(mdl.t_ds_seq[rep], mdl.y_ds_seq[rep][:,1], '-', **ds_fmt)
    
    ax1b.plot(mdl.t_ss_seq[rep], mdl.forces_ss_seq[rep][:,1] / mg, color='#000000', linewidth=3)
    ax1b.plot(mdl.t_ds_seq[rep], mdl.forces_ds_seq[rep][:,1] / mg, color='#000000', linewidth=3)

    ax1b.plot(mdl.t_ss_seq[rep], mdl.forces_ss_seq[rep][:,4]/ mg, color='#909090', linestyle='-', linewidth=3)
    ax1b.plot(mdl.t_ds_seq[rep], mdl.forces_ds_seq[rep][:,4]/ mg, color='#909090', linestyle='-', linewidth=3)
    


ax1.set_title('vertical motion', fontdict=titlefont)
ax1.set_ylim(.45, 1.05)
ax1.set_yticks(arange(3)*.1 + .8)
ax1b.set_yticks(arange(6) * .25)
ax1b.set_ylim(0, 2)
ax1b.plot(array(ax1b.get_xlim())*.99, [1, 1], 'k--')
ax1b.set_ylabel('vertical force [bodyweight]', fontdict=titlefont)
ax1.set_xlabel('time [s]', fontdict=titlefont)
ax1.set_ylabel('vertical CoM position', fontdict=titlefont)
ax0.set_xlim([dat_ss[0][0,0]-.15, dat_ds[nsteps-1][-1,0]+.25])
ax0.set_ylim([min_foot - .05, max_foot + .05])
subplots_adjust(hspace=.25, top=.95, bottom=.08)

fig.savefig('tmp/sol_b1.pdf')
pass



In [133]:

    
mdl.feet1_seq[::2][rep]









    Out[133]:





[0, 0, 0]



In [134]:

    
foot1 = dot(rotmat, array(mdl.feet1_seq[::2][rep])[[0,2]])



In [113]:

    
P









    Out[113]:





{'delta_beta': 0.0,
 'foot1': [0, 0, 0],
 'foot2': [-2, 0, 0],
 'g': [0, -9.81, 0],
 'lp1': [13000.0, 1.0, 1.1876965559821411, 0.050000000000000003],
 'lp2': [15000.0, 1.0, 1.2208578117700335, -0.050000000000000003],
 'm': 80}



In [92]:

    
dat[idx]









    Out[92]:





[array([ 0.92397967,  0.4084825 ,  1.29894495,  0.94003792,  0.10411967]),
 array([  1.30000000e+04,   1.50000000e+04,   1.18769656e+00,
         1.22085781e+00,   5.00000000e-02,  -5.00000000e-02,
         1.00000000e+00,   1.00000000e+00]),
 3555.2907146064817,
 array([  1.62567548e-01+0.97679344j,   1.62567548e-01-0.97679344j,
         2.14347643e-05+0.j        ,   1.03531933e-01+0.j        ])]



In [93]:

    
sqrt(1.2263**2 + .40848**2 + .128**2)









    Out[93]:





1.2988655051236058



In [ ]: